#!/usr/bin/env python3

import os
os.environ["QT_X11_NO_MITSHM"] = "1"  # 必须在导入PyQt5之前设置

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import String
from std_msgs.msg import Bool
from cv_bridge import CvBridge
import socket
import threading
import cv2
from detection_gui.scripts.GUI import GUI
import sys
from detection_gui.scripts.yolo_detector import YOLODetector
from PyQt5.QtWidgets import QApplication  # 新增导入
from manipulation.msg import ManiProcessCmd
import os

from pathlib import Path  # 新增这一行（确保在文件开头的导入区）
#print(os.environ.get("QT_QPA_PLATFORM_PLUGIN_PATH"))
os.environ.pop("QT_QPA_PLATFORM_PLUGIN_PATH", None)  # 确保环境变量被清除




class Ros2Interface(Node):
    def __init__(self, debug_mode=False, video_file=None):
        super().__init__('ros2_interface_node')

        # 新增：解析命令行参数
        self.declare_parameter('date', '1.1')  # 默认值
        self.declare_parameter('i', 1)         # 默认值
        self.received_date = self.get_parameter('date').get_parameter_value().string_value
        self.received_i = self.get_parameter('i').get_parameter_value().integer_value
        
        self.get_logger().info(f"接收参数: date={self.received_date}, i={self.received_i}")


        self.get_logger().info('ROS2 Interface Node has been started.')
        self.bridge = CvBridge()
        self.debug_mode = debug_mode
        self.video_file = video_file

        # 新增：视频录制相关变量
        self.video_writer = None  # 视频写入器
        self.recording_started = False  # 录制状态标志

        # 模型路径和类别信息
        self.yolo_detector = YOLODetector()

        # 订阅视频流
        self.video_sub = self.create_subscription(
            Image,
            '/camera/color/image_raw',
            self.listener_callback,
            10
        )

        # 新增：存储t3接收的参
        # 订阅t3话题（字符串label）
        self.t3_sub = self.create_subscription(
            String,
            '/view_pose',
            self.t3_callback,
            10
        )

        # 创建Pause消息发布者
        self.mani_pub = self.create_publisher(
            ManiProcessCmd,
            '/mani_process2',
            10
        )

        self.is_paused = False  # 暂停标志
        self.gui: GUI = None  # 新增：用于存储GUI实例

        # 创建Unix Domain Socket服务器
        self.server_address = '/tmp/ros2_gui_socket'
        self.socket_server = None
        self.start_socket_server()


    # t3回调：接收label并调用GUI切换界面
    def t3_callback(self, msg):
        label = msg.data.strip().lower()
        self.get_logger().info(f"Received label from t3: {label}")
        if self.gui and (label in self.gui.names or label == "move"):
            # 切换GUI界面（通过信号确保线程安全，无需修改GUI）
            self.gui.switch_current_surface(label)

    # 新增：发布合格状态的方法
    def publish_qualify_status(self, status):
        # 创建ManiProcessCmd消息，适配字段结构
        msg = ManiProcessCmd()
        msg.level1_name = "place"  # 用字符串标识状态类型
        msg.level2_name = "move"  # 无二级名称，留空
        msg.value = 1.0 if status else 0.0  # 布尔值转数值（1.0=合格，0.0=不合格）
        
        self.mani_pub.publish(msg)
        self.get_logger().info(
            f"发布合格状态到/mani_process2: level1_name={msg.level1_name}, value={msg.value}"
        )

    def set_gui(self, gui):
        self.gui = gui
        self.gui.ros2_node = self  # 让GUI能调用发布方法

    def publish_video_frame(self):
        ret, frame = self.video_cap.read()
        if ret:
            img_msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")
            self.video_pub.publish(img_msg)
            # self.get_logger().info(f'Published video frame with width: {frame.shape[1]}, height: {frame.shape[0]}')
        else:
            # self.get_logger().info('End of video file reached.')
            self.timer.cancel()

    def listener_callback(self, msg):
        # 当未收到Pause指令时处理视频流
        if self.is_paused:
            return

        # 将ROS图像消息转为OpenCV格式（原始帧）
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")

        # 新增：录制原始视频流（关键逻辑）
        self.record_raw_video(cv_image)

        # 原有：将处理后的帧传给GUI
        if self.gui:
            result = self.yolo_detector(cv_image, save=False)
            print(f"[DEBUG]: fps={result['avg_fps']:.2f}")
            self.gui.set_current_result(result)
        
        # if self.gui.isPaused:
        #     self.handle_pause_command()
        #     return
        

    def record_raw_video(self, frame):
        # 确保GUI已初始化且工件文件夹存在（修正Path引用）
        if not self.gui or not hasattr(self.gui, 'workpiece_folder') or self.gui.workpiece_folder == Path(""):
            return

        # 初始化视频写入器（仅在第一帧时执行）
        if self.video_writer is None:
            # 视频保存路径：与GUI相同的工件文件夹，命名为"raw_video.avi"
            video_path = self.gui.workpiece_folder / "raw_video.avi"
            # 获取帧的宽高
            height, width = frame.shape[:2]
            # 初始化写入器（XVID编码，15fps）
            fourcc = cv2.VideoWriter_fourcc(*'XVID')
            self.video_writer = cv2.VideoWriter(str(video_path), fourcc, 15.0, (width, height))
            self.get_logger().info(f"开始录制原始视频: {video_path}")

        # 写入当前帧
        if self.video_writer.isOpened():
            self.video_writer.write(frame)

    def start_socket_server(self):
        # 创建并启动Unix Domain Socket服务器
        try:
            # 删除可能存在的旧socket文件
            if os.path.exists(self.server_address):
                os.unlink(self.server_address)

            self.socket_server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
            self.socket_server.bind(self.server_address)
            self.socket_server.listen(1)

            # 在后台线程中接受连接
            thread = threading.Thread(target=self._accept_connections, daemon=True)
            thread.start()
            self.get_logger().info(f'Socket server started on {self.server_address}')
        except Exception as e:
            self.get_logger().error(f'Error starting socket server: {str(e)}')

    def _accept_connections(self):
        while True:
            try:
                conn, addr = self.socket_server.accept()
                self.get_logger().info('Connection from client established')

                # 处理客户端消息
                data = conn.recv(1024)
                if data.decode() == 'pause':
                    self.handle_pause_command()

                conn.close()
            except Exception as e:
                self.get_logger().error(f'Error handling client connection: {str(e)}')
                break

    def handle_pause_command(self):
        # 处理暂停命令
        self.is_paused = True
        # 发布Pause消息
        pause_msg = ManiProcessCmd()
        pause_msg.level1_name = "Pause"
        self.pause_pub.publish(pause_msg)
        self.get_logger().info('Published Pause message to output_topic')
        if self.debug_mode:
            print("Published 'Pause' message as a PUBLISHER in debug mode.")

    def send_pause_command(self):
        try:
            client_socket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
            client_socket.connect(self.server_address)
            client_socket.sendall('Pause'.encode())
            client_socket.close()
        except Exception as e:
            self.get_logger().error(f'Error sending Pause command: {str(e)}')
    
    def destroy_node(self):
        # 释放视频写入器
        if self.video_writer is not None:
            self.video_writer.release()
            self.get_logger().info("原始视频录制已停止并保存")
        super().destroy_node()


def main(args=None):
    # 首先初始化 QApplication（必须在主线程中）
    app = QApplication(sys.argv)
    
    # 然后初始化 ROS2
    rclpy.init(args=args)
    ros2_interface = Ros2Interface()

    # 创建 GUI 实例（在主线程中）
    gui = GUI(ros2_interface.send_pause_command, 
          date=ros2_interface.received_date, 
          i=ros2_interface.received_i)
    ros2_interface.set_gui(gui)

    # 在后台线程中运行 ROS2 节点
    ros_thread = threading.Thread(target=rclpy.spin, args=(ros2_interface,), daemon=True)
    ros_thread.start()

    try:
        gui.show()  # 显示GUI
        exit_code = app.exec_()  # 启动应用程序的事件循环
        sys.exit(exit_code)
    except Exception as e:
        ros2_interface.get_logger().error(f'An error occurred: {str(e)}')
    finally:
        # 清理资源
        if ros2_interface.socket_server:
            if os.path.exists(ros2_interface.server_address):
                os.unlink(ros2_interface.server_address)

        if hasattr(ros2_interface, 'video_cap') and ros2_interface.video_cap.isOpened():
            ros2_interface.video_cap.release()

        ros2_interface.destroy_node()
        rclpy.shutdown()
        ros_thread.join(timeout=1.0)  # 给线程一点时间来清理


if __name__ == '__main__':
    main()